#include "sensor_msgs/Imu.h"
#include "serial/serial.h"
#include "std_msgs/String.h"
#include "ros/ros.h"
#include "odom_pub/main.h"
#include <tf/transform_broadcaster.h>


sensor_msgs::Imu IMU;
serial::Serial port;

uint8_t receive_data[MAX_interaction_byte];
// uint8_t verify_num[MAX_interaction_byte];

int data_check_head=0;
int data_check_end=0;

void data_verify(void);

void data_verify(void)
{
    for (int n = 0; n < MAX_interaction_byte; n++)
    {
        if(receive_data[n]==data_frame_head) 
        {
            data_check_head=n;      
            for (int m = 0; m < MAX_interaction_byte; m++)
            {
                if(receive_data[m]==data_frame_END)
                {
                    data_check_end=m; 
                    break;
                }
            }
            break;
        }        
    }
    for (int i = data_check_head, j = 0; i <= MAX_interaction_byte; j++)
    {
        if (j==MAX_interaction_byte) break;
        verify_num[j] = receive_data[i];
        i++;
        if (i==MAX_interaction_byte) i=0;
        
    }
    if(verify_num[0]!=0xaa||verify_num[MAX_interaction_byte-1]!=0xbb)
    {
        port.read(receive_data,MAX_interaction_byte);
        data_verify();
    }
}

int main(int argc, char *argv[])
{
    /* code */
    ros::init(argc, argv, "odom_to_baselink" );
    ros::NodeHandle nh;
    ros::Publisher pub = nh.advertise<nav_msgs::Odometry>("odom",100);
    tf::TransformBroadcaster odom_broadcaster;
    try
    {
        port.setPort("/dev/ttyUSB0");
        port.setBaudrate(115200);
        serial::Timeout to = serial::Timeout::simpleTimeout(1000);
        port.setTimeout(to);
        port.open();
        ROS_INFO("port start success");
    }
    catch(serial::IOException &e)
    {
        ROS_INFO("port start failed");
        return -1;
    }

    ros::Time current_time, last_time;
    
    double pitch, yaw, roll;

    double x;
    double y;
    double th;

    double vx;
    double vy;
    double vth;

    current_time = ros::Time::now();
    last_time = ros::Time::now();

    ros::Rate r(10);
    ROS_INFO("odom_publish working");
    while(ros::ok())
    {  
        ros::spinOnce();
        current_time = ros::Time::now();         
        port.read(receive_data,MAX_interaction_byte);
        // r.sleep();
        data_verify();
        // for (int i = 0; i < 20; i++)
        // {
        //     /* code */
        //     // ROS_INFO("receive_data[%d]--%d",i,receive_data[i]);
        //     ROS_INFO("verify_num[%d]--%d",i,verify_num[i]);
        // }
        // ros::spinOnce();


        
        vx = (double)verify_num[6]+(double)verify_num[7]/100.0f;
        vy = (double)verify_num[8]+(double)verify_num[9]/100.0f;
        vth = (double)verify_num[10]+(double)verify_num[11]/100.0f;


        if(verify_num[12]==0) vx = 0-vx;
        if(verify_num[13]==0) vy = 0-vy;
        if(verify_num[14]==0) vth = 0-vth;

        // ROS_INFO("vx: %f", vx);
        // ROS_INFO("vy: %f", vy);
        // ROS_INFO("vth: %f", vth);

        double dt = (current_time - last_time).toSec();
        double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
        double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
        double delta_th = vth * dt;


            pitch=verify_num[1];
            yaw=verify_num[3];
            roll=verify_num[5];

            if (pitch > 180) pitch -= 255;
            if (yaw > 180) yaw -= 255;
            if (roll > 180) roll -= 255;


        
        // ROS_INFO("pitch:  %f",pitch);
        // ROS_INFO("yaw:  %f",yaw);
        // ROS_INFO("roll:  %f",roll);

        // quat = tf::createQuaternionMsgFromRollPitchYaw((roll*3.14159/180),(pitch*3.14159/180),(yaw*3.14159/180));

        x += delta_x;
        y += delta_y;
        th += delta_th;

         geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw((yaw*3.14159/180));

        geometry_msgs::TransformStamped odom_trans;
        odom_trans.header.stamp = current_time;
        odom_trans.header.frame_id = "odom";
        odom_trans.child_frame_id = "base_link";

        odom_trans.transform.translation.x = x;
        odom_trans.transform.translation.y = y;
        odom_trans.transform.translation.z = 0.0;

        odom_trans.transform.rotation = odom_quat;
        odom_broadcaster.sendTransform(odom_trans);

        //next, we'll publish the odometry message over ROS
        odom.header.stamp = current_time;
        odom.header.frame_id = "odom";
        odom.child_frame_id = "base_link";


        //设置位置
        odom.pose.pose.position.x = x;
        odom.pose.pose.position.y = y;
        odom.pose.pose.position.z = 0.0;
        //设置欧拉角
        odom.pose.pose.orientation = odom_quat;

        // ROS_INFO("x-%f, y-%f, z-%f, w-%f", quat.x, quat.y, quat.z, quat.w);

        //设置速度
        odom.twist.twist.linear.x = vx;
        odom.twist.twist.linear.y = vy;
        odom.twist.twist.angular.z = vth;

        last_time = current_time;
        pub.publish(odom);
        r.sleep();
        
    }

    
    return 0;
}
